//
// File: only_vehicle_model.cpp
//
// Code generated for Simulink model 'only_vehicle_model'.
//
// Model version                  : 1.4
// Simulink Coder version         : 9.9 (R2023a) 19-Nov-2022
// C/C++ source code generated on : Sun Dec 10 22:42:43 2023
//
// Target selection: ert.tlc
// Embedded hardware selection: Intel->x86-64 (Windows64)
// Code generation objectives: Unspecified
// Validation result: Not run
//
#include "only_vehicle_model.h"
#include <cmath>
#include "rtwtypes.h"
#include "only_vehicle_model_private.h"

// Exported block signals
real_T total_lon_force_front_axle;     // '<Root>/In1'
real_T total_lon_force_rear_axle;      // '<Root>/In2'
real_T road_grade;                     // '<Root>/In3'

//
// This function updates continuous states using the ODE3 fixed-step
// solver algorithm
//
void only_vehicle_model::rt_ertODEUpdateContinuousStates(RTWSolverInfo *si )
{
  // Solver Matrices
  static const real_T rt_ODE3_A[3]{
    1.0/2.0, 3.0/4.0, 1.0
  };

  static const real_T rt_ODE3_B[3][3]{
    { 1.0/2.0, 0.0, 0.0 },

    { 0.0, 3.0/4.0, 0.0 },

    { 2.0/9.0, 1.0/3.0, 4.0/9.0 }
  };

  time_T t { rtsiGetT(si) };

  time_T tnew { rtsiGetSolverStopTime(si) };

  time_T h { rtsiGetStepSize(si) };

  real_T *x { rtsiGetContStates(si) };

  ODE3_IntgData *id { static_cast<ODE3_IntgData *>(rtsiGetSolverData(si)) };

  real_T *y { id->y };

  real_T *f0 { id->f[0] };

  real_T *f1 { id->f[1] };

  real_T *f2 { id->f[2] };

  real_T hB[3];
  int_T i;
  int_T nXc { 3 };

  rtsiSetSimTimeStep(si,MINOR_TIME_STEP);

  // Save the state values at time t in y, we'll use x as ynew.
  (void) std::memcpy(y, x,
                     static_cast<uint_T>(nXc)*sizeof(real_T));

  // Assumes that rtsiSetT and ModelOutputs are up-to-date
  // f0 = f(t,y)
  rtsiSetdX(si, f0);
  only_vehicle_model_derivatives();

  // f(:,2) = feval(odefile, t + hA(1), y + f*hB(:,1), args(:)(*));
  hB[0] = h * rt_ODE3_B[0][0];
  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0]);
  }

  rtsiSetT(si, t + h*rt_ODE3_A[0]);
  rtsiSetdX(si, f1);
  this->step();
  only_vehicle_model_derivatives();

  // f(:,3) = feval(odefile, t + hA(2), y + f*hB(:,2), args(:)(*));
  for (i = 0; i <= 1; i++) {
    hB[i] = h * rt_ODE3_B[1][i];
  }

  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1]);
  }

  rtsiSetT(si, t + h*rt_ODE3_A[1]);
  rtsiSetdX(si, f2);
  this->step();
  only_vehicle_model_derivatives();

  // tnew = t + hA(3);
  // ynew = y + f*hB(:,3);
  for (i = 0; i <= 2; i++) {
    hB[i] = h * rt_ODE3_B[2][i];
  }

  for (i = 0; i < nXc; i++) {
    x[i] = y[i] + (f0[i]*hB[0] + f1[i]*hB[1] + f2[i]*hB[2]);
  }

  rtsiSetT(si, tnew);
  rtsiSetSimTimeStep(si,MAJOR_TIME_STEP);
}

// Model step function
void only_vehicle_model::step()
{
  real_T rtb_Add6;
  real_T rtb_VectorConcatenate5_0;
  if (rtmIsMajorTimeStep((&only_vehicle_model_M))) {
    // set solver stop time
    rtsiSetSolverStopTime(&(&only_vehicle_model_M)->solverInfo,
                          (((&only_vehicle_model_M)->Timing.clockTick0+1)*
      (&only_vehicle_model_M)->Timing.stepSize0));
  }                                    // end MajorTimeStep

  // Update absolute time of base rate at minor time step
  if (rtmIsMinorTimeStep((&only_vehicle_model_M))) {
    (&only_vehicle_model_M)->Timing.t[0] = rtsiGetT(&(&only_vehicle_model_M)
      ->solverInfo);
  }

  // Integrator: '<S5>/Integrator'
  // Unit Conversion - from: deg to: rad
  // Expression: output = (0.0174533*input) + (0)
  only_vehicle_model_B.Integrator = only_vehicle_model_X.Integrator_CSTATE;

  // Sum: '<S42>/Add1' incorporates:
  //   SignalConversion generated from: '<S5>/Vector Concatenate5'

  rtb_VectorConcatenate5_0 = only_vehicle_model_B.Integrator -
    only_vehicle_model_ConstB.UnaryMinus[0];

  // Sqrt: '<S42>/Sqrt' incorporates:
  //   Product: '<S42>/Product'
  //   Sum: '<S42>/Add1'
  //   Sum: '<S42>/Sum of Elements'

  rtb_Add6 = std::sqrt(((0.0 - only_vehicle_model_ConstB.UnaryMinus[1]) * (0.0 -
    only_vehicle_model_ConstB.UnaryMinus[1]) + rtb_VectorConcatenate5_0 *
                        rtb_VectorConcatenate5_0) + (0.0 -
    only_vehicle_model_ConstB.UnaryMinus[2]) * (0.0 -
    only_vehicle_model_ConstB.UnaryMinus[2]));

  // Gain: '<S5>/1//m' incorporates:
  //   Constant: '<S1>/AirTempConstant'
  //   Constant: '<S1>/FExtConstant'
  //   Gain: '<S42>/.5.*A.*Pabs.//R.//T'
  //   Gain: '<S42>/4'
  //   Inport: '<Root>/In1'
  //   Inport: '<Root>/In2'
  //   Inport: '<Root>/In3'
  //   Product: '<S42>/Product1'
  //   Product: '<S42>/Product2'
  //   Product: '<S42>/Product3'
  //   Product: '<S5>/Product1'
  //   Sum: '<S42>/Add1'
  //   Sum: '<S5>/Add'
  //   Trigonometry: '<S42>/Tanh'
  //   Trigonometry: '<S5>/Trigonometric Function2'
  //   UnitConversion: '<S2>/Unit Conversion'

  only_vehicle_model_B.xddot = ((((0.0 - std::sin(0.017453292519943295 *
    road_grade) * only_vehicle_model_ConstB.Fz) + total_lon_force_front_axle) +
    total_lon_force_rear_axle) - rtb_Add6 * rtb_Add6 * 0.3 / 273.0 *
    705.95489413289306 * std::tanh(4.0 * rtb_VectorConcatenate5_0)) *
    0.00066666666666666664;

  // SignalConversion generated from: '<S2>/Vector Concatenate2' incorporates:
  //   Concatenate: '<S2>/Vector Concatenate2'

  only_vehicle_model_B.VectorConcatenate2[0] = only_vehicle_model_B.Integrator;

  // SignalConversion generated from: '<S2>/Vector Concatenate2' incorporates:
  //   Concatenate: '<S2>/Vector Concatenate2'

  only_vehicle_model_B.VectorConcatenate2[1] = 0.0;

  // SignalConversion generated from: '<S2>/Vector Concatenate2' incorporates:
  //   Concatenate: '<S2>/Vector Concatenate2'

  only_vehicle_model_B.VectorConcatenate2[2] = 0.0;

  // Outport: '<Root>/vehicle_speed'
  only_vehicle_model_Y.vehicle_speed = only_vehicle_model_B.VectorConcatenate2[0];
  if (rtmIsMajorTimeStep((&only_vehicle_model_M))) {
    rt_ertODEUpdateContinuousStates(&(&only_vehicle_model_M)->solverInfo);

    // Update absolute time for base rate
    // The "clockTick0" counts the number of times the code of this task has
    //  been executed. The absolute time is the multiplication of "clockTick0"
    //  and "Timing.stepSize0". Size of "clockTick0" ensures timer will not
    //  overflow during the application lifespan selected.

    ++(&only_vehicle_model_M)->Timing.clockTick0;
    (&only_vehicle_model_M)->Timing.t[0] = rtsiGetSolverStopTime
      (&(&only_vehicle_model_M)->solverInfo);

    {
      // Update absolute timer for sample time: [0.05s, 0.0s]
      // The "clockTick1" counts the number of times the code of this task has
      //  been executed. The resolution of this integer timer is 0.05, which is the step size
      //  of the task. Size of "clockTick1" ensures timer will not overflow during the
      //  application lifespan selected.

      (&only_vehicle_model_M)->Timing.clockTick1++;
    }
  }                                    // end MajorTimeStep
}

// Derivatives for root system: '<Root>'
void only_vehicle_model::only_vehicle_model_derivatives()
{
  only_vehicle_model::XDot_only_vehicle_model_T *_rtXdot;
  _rtXdot = ((XDot_only_vehicle_model_T *) (&only_vehicle_model_M)->derivs);

  // Derivatives for Integrator: '<S5>/Integrator'
  _rtXdot->Integrator_CSTATE = only_vehicle_model_B.xddot;

  // Derivatives for Integrator: '<S2>/Integrator1'
  _rtXdot->Integrator1_CSTATE = only_vehicle_model_B.Integrator;

  // Derivatives for Integrator: '<S13>/Integrator3'
  _rtXdot->Integrator3_CSTATE = only_vehicle_model_B.VectorConcatenate2[0];
}

// Model initialize function
void only_vehicle_model::initialize()
{
  // Registration code
  {
    // Setup solver object
    rtsiSetSimTimeStepPtr(&(&only_vehicle_model_M)->solverInfo,
                          &(&only_vehicle_model_M)->Timing.simTimeStep);
    rtsiSetTPtr(&(&only_vehicle_model_M)->solverInfo, &rtmGetTPtr
                ((&only_vehicle_model_M)));
    rtsiSetStepSizePtr(&(&only_vehicle_model_M)->solverInfo,
                       &(&only_vehicle_model_M)->Timing.stepSize0);
    rtsiSetdXPtr(&(&only_vehicle_model_M)->solverInfo, &(&only_vehicle_model_M
                 )->derivs);
    rtsiSetContStatesPtr(&(&only_vehicle_model_M)->solverInfo, (real_T **)
                         &(&only_vehicle_model_M)->contStates);
    rtsiSetNumContStatesPtr(&(&only_vehicle_model_M)->solverInfo,
      &(&only_vehicle_model_M)->Sizes.numContStates);
    rtsiSetNumPeriodicContStatesPtr(&(&only_vehicle_model_M)->solverInfo,
      &(&only_vehicle_model_M)->Sizes.numPeriodicContStates);
    rtsiSetPeriodicContStateIndicesPtr(&(&only_vehicle_model_M)->solverInfo,
      &(&only_vehicle_model_M)->periodicContStateIndices);
    rtsiSetPeriodicContStateRangesPtr(&(&only_vehicle_model_M)->solverInfo,
      &(&only_vehicle_model_M)->periodicContStateRanges);
    rtsiSetErrorStatusPtr(&(&only_vehicle_model_M)->solverInfo,
                          (&rtmGetErrorStatus((&only_vehicle_model_M))));
    rtsiSetRTModelPtr(&(&only_vehicle_model_M)->solverInfo,
                      (&only_vehicle_model_M));
  }

  rtsiSetSimTimeStep(&(&only_vehicle_model_M)->solverInfo, MAJOR_TIME_STEP);
  (&only_vehicle_model_M)->intgData.y = (&only_vehicle_model_M)->odeY;
  (&only_vehicle_model_M)->intgData.f[0] = (&only_vehicle_model_M)->odeF[0];
  (&only_vehicle_model_M)->intgData.f[1] = (&only_vehicle_model_M)->odeF[1];
  (&only_vehicle_model_M)->intgData.f[2] = (&only_vehicle_model_M)->odeF[2];
  (&only_vehicle_model_M)->contStates = ((X_only_vehicle_model_T *)
    &only_vehicle_model_X);
  rtsiSetSolverData(&(&only_vehicle_model_M)->solverInfo, static_cast<void *>(&(
    &only_vehicle_model_M)->intgData));
  rtsiSetIsMinorTimeStepWithModeChange(&(&only_vehicle_model_M)->solverInfo,
    false);
  rtsiSetSolverName(&(&only_vehicle_model_M)->solverInfo,"ode3");
  rtmSetTPtr((&only_vehicle_model_M), &(&only_vehicle_model_M)->Timing.tArray[0]);
  (&only_vehicle_model_M)->Timing.stepSize0 = 0.05;

  // InitializeConditions for Integrator: '<S5>/Integrator'
  only_vehicle_model_X.Integrator_CSTATE = 0.0;

  // InitializeConditions for Integrator: '<S2>/Integrator1'
  only_vehicle_model_X.Integrator1_CSTATE = 0.0;

  // InitializeConditions for Integrator: '<S13>/Integrator3'
  only_vehicle_model_X.Integrator3_CSTATE = 0.0;
}

// Model terminate function
void only_vehicle_model::terminate()
{
  // (no terminate code required)
}

// Constructor
only_vehicle_model::only_vehicle_model() :
  only_vehicle_model_Y(),
  only_vehicle_model_B(),
  only_vehicle_model_X(),
  only_vehicle_model_M()
{
  // Currently there is no constructor body generated.
}

// Destructor
// Currently there is no destructor body generated.
only_vehicle_model::~only_vehicle_model() = default;

// Real-Time Model get method
only_vehicle_model::RT_MODEL_only_vehicle_model_T * only_vehicle_model::getRTM()
{
  return (&only_vehicle_model_M);
}

//
// File trailer for generated code.
//
// [EOF]
//
